/*==================================================================================================
*   Project              : RTD AUTOSAR 4.4
*   Platform             : CORTEXM
*   Peripheral           :
*   Dependencies         : none
*
*   Autosar Version      : 4.4.0
*   Autosar Revision     : ASR_REL_4_4_REV_0000
*   Autosar Conf.Variant :
*   SW Version           : 2.0.0
*   Build Version        : S32K3_RTD_2_0_0_D2203_ASR_REL_4_4_REV_0000_20220331
*
*   (c) Copyright 2020 - 2022 NXP Semiconductors
*   All Rights Reserved.
*
*   NXP Confidential. This software is owned or controlled by NXP and may only be
*   used strictly in accordance with the applicable license terms. By expressly
*   accepting such terms or by downloading, installing, activating and/or otherwise
*   using the software, you are agreeing that you have read, and that you agree to
*   comply with and are bound by, such license terms. If you do not agree to be
*   bound by the applicable license terms, then you may not retain, install,
*   activate or otherwise use the software.
==================================================================================================*/

#include "Platform_Types.h"
#include "Mcal.h"

#include <armv7m_isa.h>
#include <arch_irq.h>
#include <xwos/ospl/skd.h>
#include <arch_skd.h>
#include <xwos/ospl/syshwt.h>

#ifdef MCAL_ENABLE_USER_MODE_SUPPORT
void Suspend_Interrupts(void);
void Resume_Interrupts(void);
#endif

__xwbsp_isr
void nmi_handler(void)
{
        while (true) {
        }
}

__xwbsp_isr __xwcc_naked
void svc_handler(void)
{
        __asm__ volatile(".syntax       unified");
        __asm__ volatile("      clrex");
        __asm__ volatile("      tst     lr, #0x4"); /* update Z flag in EPSR */
        __asm__ volatile("      ite     eq\n"
                         "      mrseq   r0, msp\n" /* if Z == 1 (bit is 0), use MSP */
                         "      mrsne   r0, psp"); /* if Z == 0 (bit is 1), use PSP */
        __asm__ volatile("      ldr     r1, [r0, #24]"); /* get old pc value */
        __asm__ volatile("      ldrb    r2, [r1, #-2]"); /* get svc number */
        __asm__ volatile("      tbh     [pc, r2, lsl #1]"); /* switch (r2) */
        __asm__ volatile("svc_table:"); /* Number of entry must be even. */
        __asm__ volatile(".2byte        (svc_0 - svc_table) / 2");
        __asm__ volatile(".2byte        (svc_1 - svc_table) / 2");
        __asm__ volatile(".2byte        (svc_2 - svc_table) / 2");
        __asm__ volatile(".2byte        (svc_3 - svc_table) / 2");
        __asm__ volatile(".2byte        (svc_4 - svc_table) / 2");
        __asm__ volatile(".2byte        (svc_5 - svc_table) / 2");
        __asm__ volatile(".2byte        (svc_6 - svc_table) / 2");
        __asm__ volatile(".2byte        (svc_7 - svc_table) / 2");
        __asm__ volatile(".2byte        (svc_8 - svc_table) / 2");
        __asm__ volatile(".2byte        (svc_9 - svc_table) / 2");
        __asm__ volatile(".2byte        (svc_10 - svc_table) / 2");
        __asm__ volatile(".2byte        (svc_11 - svc_table) / 2");
        __asm__ volatile(".2byte        (svc_12 - svc_table) / 2");
        __asm__ volatile(".2byte        (svc_13 - svc_table) / 2");
        __asm__ volatile(".2byte        (svc_14 - svc_table) / 2");
        __asm__ volatile(".2byte        (svc_15 - svc_table) / 2");
        __asm__ volatile(".align        2");
        __asm__ volatile("      bx      lr");
        __asm__ volatile("svc_0:"); /* case 0: Set Supervisor mode for Thread mode */
#ifdef MCAL_ENABLE_USER_MODE_SUPPORT
        __asm__ volatile("      mov     r0, #0x0");
        __asm__ volatile("      msr     CONTROL, r0");
#endif
        __asm__ volatile("      bx      lr");
        __asm__ volatile("svc_1:"); /* case 1: Set User mode for Thread mode */
#ifdef MCAL_ENABLE_USER_MODE_SUPPORT
        __asm__ volatile("      mov     r0, #0x1");   /*  */
        __asm__ volatile("      msr     CONTROL, r0");
#endif
        __asm__ volatile("      bx      lr");
        __asm__ volatile("svc_2:"); /* case 2: Resume all interrupts */
#ifdef MCAL_ENABLE_USER_MODE_SUPPORT
        __asm__ volatile("      b       Resume_Interrupts");
#else
        __asm__ volatile("      bx      lr");
#endif
        __asm__ volatile("svc_3:"); /* case 3: Suspend all interrupts */
#ifdef MCAL_ENABLE_USER_MODE_SUPPORT
        __asm__ volatile("      b       Suspend_Interrupts");
#else
        __asm__ volatile("      bx      lr");
#endif
        __asm__ volatile("svc_4:"); /* case 4: */
        __asm__ volatile("      bx      lr");
        __asm__ volatile("svc_5:"); /* case 5: */
        __asm__ volatile("      bx      lr");
        __asm__ volatile("svc_6:"); /* case 6: */
        __asm__ volatile("      bx      lr");
        __asm__ volatile("svc_7:"); /* case 7: */
        __asm__ volatile("      bx      lr");
        __asm__ volatile("svc_8:"); /* case 8: XWSC */
        __asm__ volatile("      b       arch_svc_xwsc");
        __asm__ volatile("svc_9:"); /* case 9: Start XWOS */
        __asm__ volatile("      ldr     r0, [r0, #0]"); /* get old r0 */
        __asm__ volatile("      b       arch_svc_skd_start");
        __asm__ volatile("svc_10:"); /* case 10: Freeze Thread */
        __asm__ volatile("      push    {r0, lr}");
        __asm__ volatile("      ldr     r0, [r0, #0]"); /* get old r0 value: cthd */
        __asm__ volatile("      bl      xwosplcb_thd_freeze_lic");
        __asm__ volatile("      mov     r1, r0");
        __asm__ volatile("      pop     {r0, lr}");
        __asm__ volatile("      str     r1, [r0, #0]"); /* save the return value */
        __asm__ volatile("      bx      lr");
        __asm__ volatile("svc_11:"); /* case 11: thread exits */
        __asm__ volatile("      push    {r0, lr}");
        __asm__ volatile("      ldr     r1, [r0, #4]"); /* get old r1 value */
        __asm__ volatile("      ldr     r0, [r0, #0]"); /* get old r0 value */
        __asm__ volatile("      bl      xwosplcb_thd_exit_lic");
        __asm__ volatile("      mov     r1, r0");
        __asm__ volatile("      pop     {r0, lr}");
        __asm__ volatile("      str     r1, [r0, #0]"); /* save the return value */
        __asm__ volatile("      bx      lr");
        __asm__ volatile("svc_12:"); /* case 12: Suspend scheduler */
        __asm__ volatile("      push    {r0, lr}");
        __asm__ volatile("      ldr     r0, [r0, #0]"); /* get old r0 value: xwskd */
        __asm__ volatile("      bl      xwosplcb_skd_suspend_lic");
        __asm__ volatile("      mov     r1, r0");
        __asm__ volatile("      pop     {r0, lr}");
        __asm__ volatile("      str     r1, [r0, #0]"); /* save the return value */
        __asm__ volatile("      bx      lr");
        __asm__ volatile("svc_13:"); /* case 13: Resume scheduler */
        __asm__ volatile("      push    {r0, lr}");
        __asm__ volatile("      ldr     r0, [r0, #0]"); /* get old r0 value: xwskd */
        __asm__ volatile("      bl      xwosplcb_skd_resume_lic");
        __asm__ volatile("      mov     r1, r0");
        __asm__ volatile("      pop     {r0, lr}");
        __asm__ volatile("      str     r1, [r0, #0]"); /* save the return value */
        __asm__ volatile("      bx      lr");
        __asm__ volatile("svc_14:"); /* case 14: Migrate thread */
#if defined(XWCFG_CORE__mp)
        __asm__ volatile("      push    {r0, lr}");
        __asm__ volatile("      ldr     r1, [r0, #4]"); /* get old r1 value */
        __asm__ volatile("      ldr     r0, [r0, #0]"); /* get old r0 value */
        __asm__ volatile("      bl      xwosplcb_thd_outmigrate_lic");
        __asm__ volatile("      mov     r1, r0");
        __asm__ volatile("      pop     {r0, lr}");
        __asm__ volatile("      str     r1, [r0, #0]"); /* save the return value */
#endif
        __asm__ volatile("      bx      lr");
        __asm__ volatile("svc_15:"); /* case 15: */
        __asm__ volatile("      bx      lr");
}

__xwbsp_isr
void undefined_handler(void)
{
        while (true) {
        }
}

#define PLATFORM_START_SEC_CODE
#include "Platform_MemMap.h"

#ifdef MCAL_ENABLE_USER_MODE_SUPPORT
void Suspend_Interrupts(void)
{
    ASM_KEYWORD("push {r0}");
    ASM_KEYWORD("mov   r0, #0x10");
    ASM_KEYWORD(" msr BASEPRI, r0");
    ASM_KEYWORD("pop {r0}");
}

void Resume_Interrupts(void)
{
    ASM_KEYWORD("push {r0}");
    ASM_KEYWORD("mov   r0, #0x0");
    ASM_KEYWORD("msr BASEPRI, r0");
    ASM_KEYWORD("pop {r0}");
}
#endif

#define PLATFORM_STOP_SEC_CODE
#include "Platform_MemMap.h"
